#include "Breach.h"
#include "../Commands/AutoBreach.h"
#include "../Robotmap.h"
#include "../NetworkMap.h"
Breach::Breach() : Subsystem("Breach"), breachSolenoid(BREACH_SOLENOID), breachSolenoidDown(BREACH_SOLENOID_DOWN) {
	isBreachUp = false;
	isBreachPulsing = false;
	
}

void Breach::Init()
{
	breachIsDown = new Notifier((TimerEventHandler)(Breach::NotifyDown), (void*)this);
	breachNUp = new Notifier((TimerEventHandler)(Breach::MoveUp), (void*)this);
	breachNDown = new Notifier((TimerEventHandler)(Breach::MoveDown), (void*)this);
	breachPulse = new Notifier((TimerEventHandler)(Breach::Pulse), (void*)this);
	stopPulsing = new Notifier((TimerEventHandler)(Breach::StopPulsing), (void*)this);
	startPulsing = new Notifier((TimerEventHandler)(Breach::StartPulsing), (void*)this);
}

void Breach::Pulse(void *handle)
{
	Breach* sys = (Breach*) handle;
	if (sys->isBreachUp) { return; }
	sys->isBreachUp = true;
	sys->breachNUp->StartSingle(0.1);
	sys->breachNDown->StartSingle(0.6);	
}

void Breach::InitDefaultCommand() {
	// Set the default command for a subsystem here.
	//SetDefaultCommand(new MySpecialCommand());
	SetDefaultCommand(new AutoBreach());
}

void Breach::MoveUp(void *thebreach) {
	Breach *lbreach = (Breach*) thebreach;
	lbreach->ControlBreachState(kBreachUp);
}

void Breach::MoveDown(void *thebreach) {
	Breach *lbreach = (Breach*) thebreach;
	lbreach->ControlBreachState(kBreachDown);
}

void Breach::NotifyDown(void* b)
{
	Breach* breachsys = (Breach*) b;
	breachsys->isBreachUp = false;
}

void Breach::StartPulsing(void* h)
{
	Breach* b = (Breach*)h;
	b->isBreachPulsing = true;
	Breach::Pulse(b);
	b->breachPulse->StartPeriodic(1.0);
}

void Breach::StopPulsing(void* h)
{
	Breach* b = (Breach*)h;
	b->isBreachPulsing = false;
	b->breachPulse->Stop();
}
void Breach::ControlBreachState(BreachDirection Direction){
	if (Direction == kBreachUp){
		isBreachUp = true;
		breachSolenoid.Set(true);
		breachSolenoidDown.Set(false);
		CommandBase::client->SetNWInt(brchSol, 1);
	} else {
		CommandBase::client->SetNWInt(brchSol, 0);
		breachIsDown->StartSingle(0.6);
		breachSolenoid.Set(false);
		breachSolenoidDown.Set(true);
	}
}
bool Breach::IsBreachUp(){
	return isBreachUp;
}

// Put methods for controlling this subsystem
// here. Call these from Commands.
